#include "uav_swarms/avoid.hpp" 

#include "rclcpp/rclcpp.hpp"
 void AvoidControl::controlLoop()
    {
        // 每辆无人机计算避障控制
        for (int i = 1; i < vehicle_num; ++i)
        {
            Vector3d avoid_force(0, 0, 0);
            Vector3d temp(0, 0, 0);

            // 遍历其他无人机，计算相对距离和避障向量
            for (int j = 0; j < vehicle_num; ++j)
            {
                if (i != j)
                {
                    Vector3d pos_i(pose[i].pose.position.x, pose[i].pose.position.y, pose[i].pose.position.z);
                    Vector3d pos_j(pose[j].pose.position.x, pose[j].pose.position.y, pose[j].pose.position.z);
                    
                    temp = calculateExponentialForce(pos_i, pos_j);
                    avoid_force += temp;
                    // Vector3d diff = pos_i - pos_j;
                    // double distance = diff.norm();

                    // // 如果距离小于避障半径，计算避障向量
                    // if (distance < avoid_radius)
                    // {
                    //     Vector3d avoid_dir = diff.normalized();  // 计算避障方向
                    //     avoid_force += avoid_dir * (avoid_radius - distance);  // 避障力与距离成反比
                    // }
                }
            }

            // 将避障向量转换为 ROS 消息，并发布
            geometry_msgs::msg::Vector3 avoid_msg;
            avoid_msg.x = avoid_force.x();
            avoid_msg.y = avoid_force.y();
            avoid_msg.z = avoid_force.z();

            vehicles_avoid_control[i] = avoid_msg;
            avoid_control_pub[i]->publish(avoid_msg);
            RCLCPP_INFO(this->get_logger(), "Publish avoid control");
            RCLCPP_INFO(this->get_logger(), "drone_%d, avoid velocity is: x: %f, y: %f, z: %f",i ,avoid_force.x(), avoid_force.y(), avoid_force.z());
            // RCLCPP_INFO(this->get_logger(), "------------------------");
        }
    }